
#ifndef ISOFTCONCAVEALGORITHM_H
#define ISOFTCONCAVEALGORITHM_H

#include <BConvexHull>
#include <BSoftBody>
#include "IAlgorithm.h"
#include "IConvexConvexAlgorithm.h"
#include "IBroadphase.h"
#include "ITriangleCallback.h"
#include "IOverlapPair.h"
#include "IHashMap.h"
#include "ISoftBodyInternals.h"

namespace BWE
{
	struct TrigIndex
	{
		int m_PartIdTriangleIndex;
		BShape* m_childShape;

		TrigIndex()
		{
			m_PartIdTriangleIndex = 0;
			m_childShape = 0;
		}
		TrigIndex(int triangleIndex, BShape* shape)
		{
			m_PartIdTriangleIndex = triangleIndex;
			m_childShape = shape;
		}

		int getTriangleIndex() const
		{
			// Get only the lower bits where the triangle index is stored
			int maxNumPairs = 10;
			BUInt x = 0;
			BUInt y = (~(x & 0)) << (31 - maxNumPairs);
			return (m_PartIdTriangleIndex & ~(y));
		}
		int getPartId() const
		{
			// Get only the highest bits where the part index is stored
			int maxNumPairs = 10;
			return (m_PartIdTriangleIndex >> (31 - maxNumPairs));
		}
		int getUid() const
		{
			return m_PartIdTriangleIndex;
		}
	};

	typedef IHashKey<TrigIndex> TrigIndexHaskKey;

	class ISoftConcaveAlgorithm : public IAlgorithm
	{
	public:
		ISoftConcaveAlgorithm();
		~ISoftConcaveAlgorithm();

		virtual void caculate() override;
		virtual void timeImpact() override;

		void clearCache();

	private:
		struct ISoftBodyTriangleCallback : public ITriangleCallback
		{
			BSoftBody*	m_softBody;
			BBody*		m_trigBody;
			BSpace		m_space;
			IAlgorithm* m_result;
			IHashMap<TrigIndexHaskKey, TrigIndex> m_shapeCache;

			ISoftBodyTriangleCallback()
			{
				m_softBody = 0;
				m_trigBody = 0;
				clearCache();
			}
			~ISoftBodyTriangleCallback()
			{
				clearCache();
			}

			void setTimeStepAndCounters(BReal margin, const BMatrix& trigTransform, IAlgorithm* result)
			{
				m_result = result;

				BVector aabbMin, aabbMax;
				m_softBody->getAabb(aabbMin, aabbMax);
				BVector dimension = (aabbMax - aabbMin) * BReal(0.5);
				BVector bodyCenter = (aabbMax + aabbMin) * BReal(0.5);

				BMatrix softTransform(bodyCenter);

				BMatrix matrix = trigTransform.inverse() * softTransform;
				BVector extents = dimension + margin;
				BVector abs_vec0 = matrix.column(0).absolute();
				BVector abs_vec1 = matrix.column(1).absolute();
				BVector abs_vec2 = matrix.column(2).absolute();
				BVector center = matrix.position();
				BVector extent(extents.dot(abs_vec0), extents.dot(abs_vec1), extents.dot(abs_vec2));
				BSpace space(center - extent, center + extent);
				m_space = space;
			}

			virtual void process(const BTriangle& triangle, int triangleIndex)
			{
				TrigIndex triIndex(triangleIndex, 0);
				TrigIndexHaskKey triKey(triIndex.getUid());

				TrigIndex* shapeIndex = m_shapeCache[triKey];
				if (shapeIndex)
				{
					BShape* childShape = shapeIndex->m_childShape;
					BHolder<IAlgorithm> algorithm = new IConvexConvexAlgorithm();
					algorithm->setBodyA(m_softBody, m_softBody->shape(0), m_softBody->matrix());
					algorithm->setBodyB(m_trigBody, childShape, m_trigBody->matrix());
					algorithm->caculate();
					return;
				}

				BVector normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]);
				normal.normalize();

				BVectorArray points;
				points.append(triangle[0] + normal);
				points.append(triangle[1] + normal);
				points.append(triangle[2] + normal);
				points.append(triangle[0] - normal);
				points.append(triangle[1] - normal);
				points.append(triangle[2] - normal);

				BConvexHull* hullShape = new BConvexHull(points);
				BHolder<IAlgorithm> algorithm = new IConvexConvexAlgorithm();
				algorithm->setBodyA(m_softBody, m_softBody->shape(0), m_softBody->matrix());
				algorithm->setBodyB(m_trigBody, hullShape, m_trigBody->matrix());
				algorithm->caculate();

				triIndex.m_childShape = hullShape;
				m_shapeCache.insert(triKey, triIndex);
			}

			void clearCache()
			{
				for (int i = 0; i < m_shapeCache.size(); i++)
				{
					TrigIndex* tmp = m_shapeCache.getAtIndex(i);
					softbody_member(m_softBody)->worldInfo->sparsesdf.removeReferences(tmp->m_childShape);
					delete tmp->m_childShape;
				}
				m_shapeCache.clear();
			}

			const BSpace& space() const
			{
				return m_space;
			}

		};

		ISoftBodyTriangleCallback m_softBodyTriangleCallback;
	};

}

#endif
